#include <kutils.h>
#include <string.h>
#include "syscall.h"
#include "display.h"
#include "pit.h"

void *memcpy(void *dest, const void *src, size_t count) 
{
	u32int num_dwords = (u32int)(count / 4);
	u32int num_bytes = (u32int)(count % 4);
	u32int *dest32 = (u32int*)dest;
	u32int *src32 = (u32int*)src;
	u8int *dest8 = ((u8int*)dest) + num_dwords * 4;
	u8int *src8 = ((u8int*)src) + num_dwords * 4;

	__asm__ __volatile__("rep movsd;" : : "S"(src32), "D"(dest32), "c"(num_dwords));
	__asm__ __volatile__("rep movsb;" : : "S"(src8), "D"(dest8), "c"(num_bytes));
	return dest;
}

void *memset(void *dest, s32int val, size_t count)
{
	__asm__ __volatile__("rep stosb;" : : "a"(val), "D"(dest), "c"(count));
	return dest;
}

u16int *memsetw(u16int *dest, u16int val, size_t count)
{
    __asm__ __volatile__("rep stosw;" : : "a"(val), "D"(dest), "c"(count));
    return dest;
}

void exit(s32int ExitCode)
{
	u32int res1, success = 0;
	syscall1(SYS_EXIT, ExitCode, res1, success)
}

void wait(u32int milliseconds)
{
	pit_wait(milliseconds);
}

u8int inportb(u16int _port)
{
    u8int rv;
    __asm__ __volatile__ ("inb %1, %0" : "=a" (rv) : "dN" (_port));
    return rv;
}

void outportb(u16int _port, u8int _data)
{
    __asm__ __volatile__ ("outb %1, %0" : : "dN" (_port), "a" (_data));
}

u32int inportw(u16int _port)
{
    u32int rv;
    __asm__ __volatile__ ("inw %1, %0" : "=a" (rv) : "dN" (_port));
    return rv;
}

void outportw(u16int _port, u32int _data)
{
    __asm__ __volatile__ ("outw %1, %0" : : "dN" (_port), "a" (_data));
}

u32int inportl(u16int _port)
{
    u32int rv;
    __asm__ __volatile__ ("inl %1, %0" : "=a" (rv) : "dN" (_port));
    return rv;
}

void outportl(u16int _port, u32int _data)
{
    __asm__ __volatile__ ("outl %1, %0" : : "dN" (_port), "a" (_data));
}

void debug(void)
{
	__asm__ __volatile__ ("hlt");
}
